#ifndef DSO_FULLSYSTEM_H
#define DSO_FULLSYSTEM_H

#include <deque>
#include <vector>
#include <iostream>
#include <fstream>

#include "dso/util/NumType.h"
#include "dso/util/GlobalCalib.h"
#include "dso/util/NumType.h"
#include "dso/FullSystem/Residuals.h"
#include "dso/FullSystem/HessianBlocks.h"
#include "dso/util/FrameShell.h"
#include "dso/util/IndexThreadReduce.h"
#include "dso/util/ImageAndExposure.h"
#include "dso/OptimizationBackend/EnergyFunctional.h"
#include "dso/FullSystem/PixelSelector.h"

/** (xiang) 
 * 这个整个系统的入口函数，调用addActiveFrame插入一张新的图片
 */

namespace dso 
{

namespace IOWrap
{
    class Output3DWrapper;
}

class PixelSelector;
class PCSyntheticPoint;
class CoarseTracker;
struct FrameHessian;
struct PointHessian;
class CoarseInitializer;
struct ImmaturePointTemporaryResidual;
class ImageAndExposure;
class CoarseDistanceMap;
class EnergyFunctional;
    
// the full system interface 
class FullSystem {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    FullSystem();
    virtual ~FullSystem();

    // adds a new frame, and creates point & residual structs.
    void addActiveFrame(ImageAndExposure* image, int id);

    // marginalizes a frame. drops / marginalizes points & residuals.
    void marginalizeFrame(FrameHessian* frame);
    
    void blockUntilMappingIsFinished();
    
    // optimize all things in local window
    // called when add new key-frame
    float optimize(int mnumOptIts);
    
    void printResult(std::string file);
    void debugPlot(std::string name);
    void printFrameLifetimes();
    void setGammaFunction(float* BInv);
    
    // status variable 
    bool isLost=false;                // track lost?
    bool initFailed=false;            // initialization failed?
    bool initialized=false;           // initialized? 
    bool linearizeOperation=true;    // 

    // contains pointers to active frames
    std::vector<IOWrap::Output3DWrapper*> outputWrapper;
private:

    // opt single point
    int optimizePoint(PointHessian* point, int minObs, bool flagOOB);
    
    // optimize an immature point
    PointHessian* optimizeImmaturePoint(ImmaturePoint* point, int minObs, ImmaturePointTemporaryResidual* residuals);
    
    double linAllPointSinle(PointHessian* point, float outlierTHSlack, bool plot);
    
    // mainPipelineFunctions
    
    // called in tracking new frame 
    // 用于追踪上一个帧
    Vec4 trackNewCoarse(FrameHessian* fh);
    
    // called in tracking new keyframe, will also update the immature points in those keyframes
    // 用于追踪关键帧,并更新未成熟的地图点
    void traceNewCoarse(FrameHessian* fh);
    
    void activatePoints();
    void activatePointsMT();
    
    /** @brief try to optimize immature points and send them into the optimized memory
     * @param[in] optimized the optimized point vector
     * @param[in] toOptimize the immature points to be optimized 
     * @param[in] min,max the min and max index 
     */
    void activatePointsMT_Reductor(std::vector<PointHessian*>* optimized,std::vector<ImmaturePoint*>* toOptimize,int min, int max, Vec10* stats, int tid);
    
    void flagPointsForRemoval();
    void makeNewTraces(FrameHessian* newFrame, float* gtDepth);
    void initializeFromInitializer(FrameHessian* newFrame);
    void flagFramesForMarginalization(FrameHessian* newFH);
    void removeOutliers();
    // set precalc values.
    void setPrecalcValues();
    // solce. eventually migrate to ef.
    void solveSystem(int iteration, double lambda);
    
    // do linearization 
    Vec3 linearizeAll(bool fixLinearization);
    
    bool doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD);
    void backupState(bool backupLastStep);
    void loadSateBackup();
    double calcLEnergy();
    double calcMEnergy();
    
    void linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid);
    
    void applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid);
    void printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b);
    void debugPlotTracking();
    std::vector<VecX> getNullspaces(
            std::vector<VecX> &nullspaces_pose,
            std::vector<VecX> &nullspaces_scale,
            std::vector<VecX> &nullspaces_affA,
            std::vector<VecX> &nullspaces_affB);
    void setNewFrameEnergyTH();
    void printLogLine();
    void printEvalLine();
    void printEigenValLine();
    
    // tracking always uses the newest KF as reference.
    // create a new keyframe 
    void makeKeyFrame( FrameHessian* fh);
    
    // process a non-keyframe 
    void makeNonKeyFrame( FrameHessian* fh);
    
    // 将 coarse tracker 跟踪过的帧交给后端处理
    /** @brief send the tracked frame to backend
     * @param[in] fh the tracked frame 
     * @param[in] needKF is a new keyframe? 
     */
    void deliverTrackedFrame(FrameHessian* fh, bool needKF);
    
    // the main mapping loop
    void mappingLoop();
    
    // Logs
    std::ofstream* calibLog             =nullptr;
    std::ofstream* numsLog              =nullptr;
    std::ofstream* errorsLog            =nullptr;
    std::ofstream* eigenAllLog          =nullptr;
    std::ofstream* eigenPLog            =nullptr;
    std::ofstream* eigenALog            =nullptr;
    std::ofstream* DiagonalLog          =nullptr;
    std::ofstream* variancesLog         =nullptr;
    std::ofstream* nullspacesLog        =nullptr;
    std::ofstream* coarseTrackingLog    =nullptr;

    // statistics
    long int statistics_lastNumOptIts       =0;
    long int statistics_numDroppedPoints    =0;
    long int statistics_numActivatedPoints  =0;
    long int statistics_numCreatedPoints    =0;
    long int statistics_numForceDroppedResBwd=0;
    long int statistics_numForceDroppedResFwd=0;
    long int statistics_numMargResFwd=0;
    long int statistics_numMargResBwd=0;
    float statistics_lastFineTrackRMSE=0;

    // =================== changed by tracker-thread. protected by trackMutex ============
    std::mutex trackMutex;
    std::vector<FrameShell*> allFrameHistory;
    CoarseInitializer* coarseInitializer;
    Vec5 lastCoarseRMSE;

    // ================== changed by mapper-thread. protected by mapMutex ===============
    std::mutex mapMutex;
    std::vector<FrameShell*> allKeyFramesHistory;

    EnergyFunctional* ef;
    IndexThreadReduce<Vec10> treadReduce;

    float* selectionMap;
    PixelSelector* pixelSelector;
    CoarseDistanceMap* coarseDistanceMap;

    // all keyframes
    std::vector<FrameHessian*> frameHessians;   // ONLY changed in marginalizeFrame and addFrame.
    std::vector<PointFrameResidual*> activeResiduals;   // active ba residuals
    float currentMinActDist =2;

    CalibHessian Hcalib;    // Calibration

    std::vector<float> allResVec;
    // mutex etc. for tracker exchange.
    std::mutex coarseTrackerSwapMutex;            // if tracker sees that there is a new reference, tracker locks [coarseTrackerSwapMutex] and swaps the two.
    CoarseTracker* coarseTracker_forNewKF;          // set as as reference. protected by [coarseTrackerSwapMutex].
    CoarseTracker* coarseTracker;                   // always used to track new frames. protected by [trackMutex].
    
    float minIdJetVisTracker=-1, maxIdJetVisTracker=-1;
    float minIdJetVisDebug=-1, maxIdJetVisDebug=-1;
    // mutex for camToWorl's in shells (these are always in a good configuration).
    std::mutex shellPoseMutex;
    
    // tracking / mapping synchronization. All protected by [trackMapSyncMutex].
    std::mutex trackMapSyncMutex;
    std::condition_variable trackedFrameSignal;
    std::condition_variable mappedFrameSignal;
    std::deque<FrameHessian*> unmappedTrackedFrames;    // 已经跟踪，但无需加入后端的帧       
    int     needNewKFAfter=-1; // Otherwise, a new KF is *needed that has ID bigger than [needNewKFAfter]*.
    std::thread mappingThread;
    bool runMapping=true;
    bool needToKetchupMapping;

    int lastRefStopID=0;
};
}

#endif